#include "VisualData.h"

using namespace std;
using namespace cv;
using namespace aruco;

VisualData::VisualData(void) {
	framecounter = 0;
	vw.open("onboard.avi",-1,25.0,Size(640,480));

	//	leggo la posizone iniziale della telecamera rispetto al sistema globale
	RobotUtils::readInitialPose(initialPose);

	cout << "Angolo iniziale: " << initialPose.getTh() << endl;
	cout << "X,Y iniziali: " << initialPose.getX() << ", " << initialPose.getY() << endl; 

	//	leggo le coordinate globali dei marker
	RobotUtils::readMarkers(globalMarkers);


	//	contatore globale per sapere
	//	quanti marker ho trovato nell'intera "sessione"
	markersFound = 0;

	//	nome dei file in cui salvare le immagini di rilevamento
	markerFoundFileName = "log/MarkerFound-";

	//	leggo l'inclinazione della telecamera
	RobotUtils::readTheta(cameraRotAngle);

	cout << "Theta camera: " << cameraRotAngle << endl;

	//	imposto l'angolo letto
	KinectMotor motor;
	if(motor.Open())
	{
		motor.Move(0);
		motor.Move(cameraRotAngle);
		motor.Close();
	}

	cameraRotAngleRad = cameraRotAngle * CV_PI / 180.0;

	cameraRotAngleRad = (-cameraRotAngle/2) * CV_PI / 180.0;

	cout << "camera theta radianti " << cameraRotAngleRad << endl;

	//	matrice di rotazione per compensare la rotazione in basso della telecamera
	cameraRot = (cv::Mat_<double>(3,3)
		<<		1,						0,							0,
		0,	cos(cameraRotAngleRad),		sin(cameraRotAngleRad),
		0,	-sin(cameraRotAngleRad),	cos(cameraRotAngleRad));

	cout << "Camera rot: " << cameraRot << endl;

	if(!capture.open(CV_CAP_OPENNI)){
		cerr << "ERRORE: impossibile aprire il Kinect\n" << endl;
	}
}

VisualData::~VisualData(void) {
	capture.release();
	vw.release();
}

Vec3f VisualData::getRelativeCoor(int x, int y, Mat cloud) {

	Vec3f real;
	//	leggo i valori spaziali restituiti dal kinect
	Vec3f pixel = cloud.at<Vec3f > (y, x);
	// li salvo nel punto 3D e restituisco
	real[0] = pixel[0]*1000;
	real[1] = pixel[1]*1000;
	real[2] = pixel[2]*1000;

	return real;
}

bool VisualData::check3DPoint(Vec3f &point) const
{
	if(point[2] <= 0)
		return false;
	if(fabs(point[0]) == 0  ||
		fabs(point[1]) == 0 ||
		fabs(point[2]) == 0){

			cout << "Trovati punti troppo vicini" << endl;
			return false;
	}
	return true;
}

bool VisualData::findMarkers(std::vector<MyMarker> &markers)
{
				Mat cloud;
			Mat rgb;
	try{
		markers.clear();


		//	se capture e' aperta e ci sono marker posizionati nell'ambiente
		if (capture.isOpened() && globalMarkers.size() > 0)
		{


			capture.grab();

			capture.retrieve(cloud, CV_CAP_OPENNI_POINT_CLOUD_MAP);
			capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE);
			


			CameraParameters CamParam;
			CamParam.readFromXMLFile("camera.yml");

			//resizes the parameters to fit the size of the input image
			CamParam.resize(rgb.size());

			//Ok, let's detect
			detector.detect(rgb, detectedMarkers, CamParam, globalMarkers[0].getHeight() / 1000.0);

			if(detectedMarkers.empty()){
				vw << rgb; framecounter++;
				return false;
			}

			//draw a 3d cube in each marker if there is 3d info
			if (CamParam.isValid()) {
				for (unsigned int i = 0; i < detectedMarkers.size(); i++){
					CvDrawingUtils::draw3dCube(rgb, detectedMarkers[i], CamParam);
					detectedMarkers[i].draw(rgb, Scalar(0, 0, 255), 2);
				}
			} else {
				vw << rgb; framecounter++;
				return false;
			}
			
			//	coordinate dei 4 punti del marker trovati per disegnarli sull'immagine
			Point2f p1, p2, p3, p4;
			//	punti trovati del marker
			Vec3f pointCenter;
			Vec3f point0;
			Vec3f point1;
			Vec3f point2;
			Vec3f point3;




			for (unsigned int i = 0; i < detectedMarkers.size(); i++) {

				//	nome del file immagine
				stringstream fileName;

				//	ho trovato un nuovo marker
				++markersFound;

				p1.x = detectedMarkers[i][0].x;
				p1.y = detectedMarkers[i][0].y;
				p2.x = detectedMarkers[i][1].x;
				p2.y = detectedMarkers[i][1].y;
				p3.x = detectedMarkers[i][2].x;
				p3.y = detectedMarkers[i][2].y;
				p4.x = detectedMarkers[i][3].x;
				p4.y = detectedMarkers[i][3].y;

				//	disegno i cerchi sui 4 angoli
				circle(rgb, p1, 4, Scalar(0, 0, 0), -1);
				circle(rgb, p2, 4, Scalar(0, 255, 0), -1);
				circle(rgb, p3, 4, Scalar(0, 0, 255), -1);
				circle(rgb, p4, 4, Scalar(255, 255, 255), -1);

				//	salvo su file l'img con il marker trovato
				fileName << markerFoundFileName << markersFound << ".jpg";
				imwrite(fileName.str().c_str(), rgb);
				fileName.clear();

				//	calcolo i punti 3d associati ai 4 angoli del marker ed al centro
				pointCenter = getRelativeCoor(detectedMarkers[i].getCenter().x,
					detectedMarkers[i].getCenter().y, cloud);
				point0 = getRelativeCoor(detectedMarkers[i][0].x, detectedMarkers[i][0].y, cloud);
				point1 = getRelativeCoor(detectedMarkers[i][1].x, detectedMarkers[i][1].y, cloud);
				point2 = getRelativeCoor(detectedMarkers[i][2].x, detectedMarkers[i][2].y, cloud);
				point3 = getRelativeCoor(detectedMarkers[i][3].x, detectedMarkers[i][3].y, cloud);

				//	stampo i punti del marker
				ArLog::log(ArLog::Normal, "---	VisualData::findMarkers: Rilevamento n.%d:	---",
					markersFound);
				ArLog::log(ArLog::Normal, "VisualData::findMarkers: Marker id: %d",
					detectedMarkers[i].id);

				ArLog::log(ArLog::Normal, "VisualData::findMarkers: Center:			(%f,%f,%f)",
					pointCenter[0],
					pointCenter[1],
					pointCenter[2]);
				ArLog::log(ArLog::Normal, "VisualData::findMarkers: Upper Left:		(%f,%f,%f)",
					point0[0],
					point0[1],
					point0[2]);
				ArLog::log(ArLog::Normal, "VisualData::findMarkers: Upper Right:		(%f,%f,%f)",
					point1[0],
					point1[1],
					point1[2]);
				ArLog::log(ArLog::Normal, "VisualData::findMarkers: Lower Right:		(%f,%f,%f)",
					point2[0],
					point2[1],
					point2[2]);
				ArLog::log(ArLog::Normal, "VisualData::findMarkers: Lower Left:		(%f,%f,%f)",
					point3[0],
					point3[1],
					point3[2]);
				ArLog::log(ArLog::Normal, "---	---	---	---	---	---	---	---	---	---	--- ---	---	---");

				//	trovo il marker corrispondente all'id corrente
				bool markerFound = false;
				bool markerGood = false;
				for(vector<MyMarker>::const_iterator m = globalMarkers.begin();
					m != globalMarkers.end() && markerFound == false;
					++m)
				{
					if(m->getId() == detectedMarkers[i].id)
					{
						//	il marker rilevato e' presente nel file di configurazione
						markerFound = true;

						//	se tutti i punti rilevati rispettano le condizioni...
						if(check3DPoint(pointCenter) &&
							check3DPoint(point0) &&
							check3DPoint(point1) &&
							check3DPoint(point2) &&
							check3DPoint(point3))
						{
							//	il marker trovato rispetta le condizioni
							markerGood = true;

							MyMarker temp(m->getId(),m->getX(),m->getY(),m->getHeight());
							//	e anche le info attuali relative al sistema camera

							temp.setCenter(pointCenter);
							temp.setUpperLeft(point0);
							temp.setUpperRight(point1);
							temp.setLowerRight(point2);
							temp.setLowerLeft(point3);
							//	imposto in questo marker le info di posizione globale
							markers.push_back(temp);
						}

					}
				}

				//	non ho trovato il marker nel file di configurazione
				if(!markerFound){
					ArLog::log(ArLog::Normal,
						"VisualData::findMarkers: Marker %d rilevato ma non trovato nel file di configurazione",
						detectedMarkers[i].id);
					vw << rgb; framecounter++;
					return false;
				} else {
					//	ho trovato il marker, ma e' stato rilevato male
					if(!markerGood)
					{
						ArLog::log(ArLog::Normal,
							"VisualData::findMarkers: Marker %d rilevato e presente nel file ma non rilevato correttamente",
							detectedMarkers[i].id);
						vw << rgb; framecounter++;
						return false;
					}
				}
			}
			//	svuoto il vettore dei marker rilevati, perche' sara' riutilizzato
			detectedMarkers.clear();
		} else {
			Mat black;
			black.zeros(480,640,CV_8UC3);
			vw << black;
			return false;
		}
	} catch(exception e){
		Mat black;
			black.zeros(480,640,CV_8UC3);
			vw << black;
		return false;
	}
	vw << rgb; framecounter++;
	return true;
}

ArPose VisualData::getPoseFromMarker(const cv::Mat_<double> &P_INFc2d,
	const cv::Mat_<double> &P_SUPc2d,
	const cv::Mat_<double> &P_INF,
	const cv::Mat_<double> &P_SUP) const
{
	//	angolo che l'asse yc forma con la retta passante per i due punti del marker
	//	(questa retta e' parallela all'asse y globale)
	double thetaRad = -atan2(P_SUPc2d.at<double>(0,0) - P_INFc2d.at<double>(0,0),P_SUPc2d.at<double>(1,0) - P_INFc2d.at<double>(1,0));
	cout << "Theta rad: " << thetaRad << endl;
	double theta = thetaRad * 180.0 / CV_PI;
	cout << "Theta: " << theta << endl;
	ArLog::log(ArLog::Normal,
		"VisualData::getPoseFromMarker: angolo tra yc e retta passante per punti del marker:	%f",
		theta);

	//	matrice di rotazione per allineare il sistema camera al sistema globale
	cv::Mat rot = (cv::Mat_<double>(2,2) <<
		cos(thetaRad),	sin(thetaRad),
		-sin(thetaRad),	cos(thetaRad));

	cout << "rot: " << rot << endl;

	//	punti inferiore e superiore rispetto al sistema camera 2d ruotato
	cv::Mat_<double> P_INFc2dr;
	cv::Mat_<double> P_SUPc2dr;

	//	trovo i punti nel sistema camera ruotato
	P_INFc2dr = rot * P_INFc2d;
	P_SUPc2dr = rot * P_SUPc2d;



	ArLog::log(ArLog::Normal,"VisualData::getPoseFromMarker: P_INFc2dr:	(%f,%f)",
		P_INFc2dr(0,0),P_INFc2dr(1,0));
	ArLog::log(ArLog::Normal,"VisualData::getPoseFromMarker: P_SUPc2dr:	(%f,%f)",
		P_SUPc2dr(0,0),P_SUPc2dr(1,0));

	//	coordinate globali della telecamera rispetto ad entrambi i punti;
	//	dovrebbero essere uguali, ma ci saranno errori di rilevamento...
	cv::Mat_<double> P_CAMERA1(2,1);
	cv::Mat_<double> P_CAMERA2(2,1);
	cv::Mat_<double> P_CAMERA(2,1);

	P_CAMERA1(0,0) = P_INF(0,0) - P_INFc2dr(0,0);
	P_CAMERA1(1,0) = P_INF(1,0) - P_INFc2dr(1,0);
	P_CAMERA2(0,0) = P_SUP(0,0) - P_SUPc2dr(0,0);
	P_CAMERA2(1,0) = P_SUP(1,0) - P_SUPc2dr(1,0);

	cout << "PCAMERA" << P_CAMERA1 << endl;
	cout << "PCAMERA" << P_CAMERA2 << endl;
	cout << "PCAMERA" << P_CAMERA << endl;


	//	posso fare una media tra le due stime
	P_CAMERA(0,0) = 0.5 * P_CAMERA1(0,0) + 0.5 * P_CAMERA2(0,0);
	P_CAMERA(1,0) = 0.5 * P_CAMERA1(1,0) + 0.5 * P_CAMERA2(1,0);

	cout << "PCAMERA" << P_CAMERA << endl;


	//	c'e' una differenza di 90 gradi tra il sistema camera ed il sistema robot:
	//	nel sistema robot l'asse x e' perpendicolare all'asse delle ruote;
	//	nel sistema camera l'asse x e' parallela all'asse delle ruote
	theta = 90 - theta;

	cout << "theta " << theta << endl;

	ArLog::log(ArLog::Normal,"VisualData::getPoseFromMarker: P_CAMERA1:	(%f,%f)",
		P_CAMERA1(0,0),P_CAMERA1(1,0));
	ArLog::log(ArLog::Normal,"VisualData::getPoseFromMarker: P_CAMERA2:	(%f,%f)",
		P_CAMERA2(0,0),P_CAMERA2(1,0));
	ArLog::log(ArLog::Normal,"VisualData::getPoseFromMarker: P_CAMERA:	(%f,%f)",
		P_CAMERA(0,0),P_CAMERA(1,0));
	ArLog::log(ArLog::Normal,"VisualData::getPoseFromMarker: Theta:			%f",
		theta);


	//	posizione ed orientazione della telecamera rispetto al sistema globale
	ArPose pose(P_CAMERA(0,0),P_CAMERA(1,0),theta - initialPose.getTh());

	cout << "POSE " << pose.getX()  << ", " << pose.getY() << endl;

	return pose;
}

bool VisualData::getGlobalPose(ArPose &globalPose)
{
	bool markerFound = false;

	//	marker trovati
	std::vector<MyMarker> markers;

	//	posizioni del robot calcolate rispetto ai marker trovati
	//	(dovrebbero tutte indicare la stessa posizione)
	std::vector<ArPose> poses;

	//	se trovo almeno un marker, ricostruisco la posizione
	markerFound = findMarkers(markers);

	if(markerFound)
	{
		//	P_INFc3d	->	coordinate del punto medio inferiore rispetto al sistema camera 3d
		//	P_SUPc3d	->	coordinate del punto medio superiore rispetto al sistema camera 3d
		//	P_INFc3dr	->	coordinate del punto medio inferiore rispetto al sistema camera 3d ruotato
		//	P_SUPc3dr	->	coordinate del punto medio superiore rispetto al sistema camera 3d ruotato
		Mat_<double>	P_INFc3d(3,1);
		Mat_<double>	P_SUPc3d(3,1);
		Mat_<double>	P_INFc3dr(3,1);
		Mat_<double>	P_SUPc3dr(3,1);

		//	P_INFc2d	->	coordinate del punto medio inferiore rispetto al sistema camera 2d
		//	P_SUPc2d	->	coordinate del punto medio superiore rispetto al sistema camera 2d
		//	P_INF		->	coordinate del punto medio inferiore rispetto al sistema globale
		//	P_SUP		->	coordinate del punto medio superiore rispetto al sistema globale
		Mat_<double>	P_INFc2d(2,1);
		Mat_<double>	P_SUPc2d(2,1);
		Mat_<double>	P_INF(2,1);
		Mat_<double>	P_SUP(2,1);

		//	calcolo la posizione del robot attuale per tutti i marker trovati
		for(std::vector<MyMarker>::const_iterator m = markers.begin();
			m != markers.end();
			++m)
		{
			//	utilizzo il formato cv::Mat_ per i punti
			//	cosi' posso sfruttare il prodotto tra matrici
			//	per la trasformazione di coordinate

			Vec3f lm = m->getLowerMiddle();
			cout << "LM " << lm[0] << ", " << lm[1] << ", " << lm[2] << endl;
			P_INFc3d.at<double>(0,0) = lm[0];
			P_INFc3d.at<double>(1,0) = lm[1];
			P_INFc3d.at<double>(2,0) = -lm[2];

			Vec3f um = m->getUpperMiddle();

			P_SUPc3d.at<double>(0,0) = um[0];
			P_SUPc3d.at<double>(1,0) = um[1];
			P_SUPc3d.at<double>(2,0) = -um[2];

			cout <<"P_INF "  <<P_INFc3d << endl;
			cout << "P_SUP" << P_SUPc3d << endl;

			//	trasformazione dal sistema camera al sistema camera ruotato
			P_INFc3dr = cameraRot * P_INFc3d;
			P_SUPc3dr = cameraRot * P_SUPc3d;

			cout <<"P_INF "  <<P_INFc3dr << endl;
			cout << "P_SUP" << P_SUPc3dr << endl;

			//	assegno i punti del marker
			//	rispetto al sistema camera 2d...
			//	asse x 2d		-->		asse x 3dr
			//	asse y 2d		-->		- asse z 3dr
			//	altezza kinect	-->		- asse y 3dr
			P_INFc2d.at<double>(0,0) = P_INFc3dr.at<double>(0,0);
			P_INFc2d.at<double>(1,0) = -P_INFc3dr.at<double>(2,0);

			cout <<"P_INF "  << P_INFc2d << endl;

			P_SUPc2d.at<double>(0,0) = P_SUPc3dr.at<double>(0,0);
			P_SUPc2d.at<double>(1,0) = -P_SUPc3dr.at<double>(2,0);

			cout << "P_SUP" << P_SUPc2d << endl;

			ArLog::log(ArLog::Normal,"VisualData::getGlobalPose: P_INFc2d:	(%f,%f)",
				P_INFc2d(0,0),P_INFc2d(1,0));
			ArLog::log(ArLog::Normal,"VisualData::getGlobalPose: P_SUPc2d:	(%f,%f)",
				P_SUPc2d(0,0),P_SUPc2d(1,0));
			ArLog::log(ArLog::Normal,"VisualData::getGlobalPose: Camera height:	%f",
				-P_SUPc3dr(1,0));

			//	...e rispetto al sistema globale
			P_INF(0,0) = m->getX();
			P_INF(1,0) = m->getY() - (m->getHeight() / 2.0);

			P_SUP(0,0) = m->getX();
			P_SUP(1,0) = m->getY() + (m->getHeight() / 2.0);

			cout << "P_INF" << P_INF << endl;
			cout << "P_SUP" << P_SUP << endl;

			ArPose pose = getPoseFromMarker(P_INFc2d,P_SUPc2d,P_INF,P_SUP);
			poses.push_back(pose);
		}

		//	variabili temporanee per fare la media delle posizioni trovate
		double correctedPoseX	=	0;
		double correctedPoseY	=	0;
		double correctedPoseTh	=	0;

		for(std::vector<ArPose>::const_iterator p = poses.begin();
			p != poses.end();
			++p)
		{
			correctedPoseX	=	correctedPoseX + p->getX();
			correctedPoseY	=	correctedPoseY + p->getY();
			if(p->getTh() >= 0)
				correctedPoseTh	=	correctedPoseTh + p->getTh();
			else
				correctedPoseTh	=	correctedPoseTh + (p->getTh() + 360);
		}
		correctedPoseX	=	correctedPoseX / poses.size();
		correctedPoseY	=	correctedPoseY / poses.size();
		correctedPoseTh	=	correctedPoseTh / poses.size();

		//	imposto le coordinate trovate per la posizione globale
		globalPose.setX(correctedPoseX);
		globalPose.setY(correctedPoseY);
		globalPose.setTh(correctedPoseTh);
	}

	return markerFound;
}
